
#include "Components/algorithm.h"
#include "Components/basic.h"
#include "Components/line_patrol.h"
#include "Components/motion.h"
#include "Components/motor.h"
#include "Components/oled_hardware_i2c.h"
#include "Components/servo.h"
#include "Components/simple_json.h"
#include "Components/ATK_MS901M/atk_ms901m.h"
#include <ti/driverlib/m0p/dl_interrupt.h>

#define TAG "Main"
int main(void)
{
  SYSCFG_DL_init();
  SysTick_Init();
  uart_init();
  OLED_Init();
  servo_init();
  motion_init();

  OLED_ShowString(0, 0, "Car liner2", 16);

  log(TAG, "Init Done!");

  //_main();
  float kp = 65, ki = 0.00, kd = 0.0, m = 1000;
  int16_t speed = 100;
  while (1)
  {
    if (serial_is_available())
    {
      char *str = serial_read_string();

      log(TAG, "%s", str);

      mjson_get_float(str, "kp", &kp);
      mjson_get_float(str, "ki", &ki);
      mjson_get_float(str, "kd", &kd);
      mjson_get_float(str, "m", &m);
      mjson_get_int(str, "speed", (int *)&speed);

      my_printf("kp=%f ki=%f kd=%f m=%f speed=%d\n", kp, ki, kd, m, speed);
    }
  }
}
